
#include "tb67h450.h"

FastSinToDac phaseA; // A 相电流控制
FastSinToDac phaseB; // B 相电流控制

void TB67H450_DacOutputVoltage(unsigned short _voltageA_3300mVIn12bits, unsigned short _voltageB_3300mVIn12bits)
{
    OUT_PWM_TO_DAC_A((_voltageA_3300mVIn12bits >> 2));
    OUT_PWM_TO_DAC_B((_voltageB_3300mVIn12bits >> 2));
}

void TB67H450_SetInputA(unsigned char _statusAp, unsigned char _statusAm)
{
    _statusAp ? OUT_AP_H() : OUT_AP_L();
    _statusAm ? OUT_AM_H() : OUT_AM_L();
}

void TB67H450_SetInputB(unsigned char _statusBp, unsigned char _statusBm)
{
    _statusBp ? OUT_BP_H() : OUT_BP_L();
    _statusBm ? OUT_BM_H() : OUT_BM_L();
}

void TB67H450_SetSleep()
{
    phaseA.dacValue12Bits = 0;
    phaseB.dacValue12Bits = 0;

    TB67H450_SetTwoCoilsCurrent(phaseA.dacValue12Bits, phaseB.dacValue12Bits);

    TB67H450_SetInputA(false, false);
    TB67H450_SetInputB(false, false);
}

/**
  * @brief  设置驱动刹车
  * @param  NULL
  * @retval NULL
  */
void TB67H450_SetBrake()
{
    phaseA.dacValue12Bits = 0;
    phaseB.dacValue12Bits = 0;

    TB67H450_SetTwoCoilsCurrent(phaseA.dacValue12Bits, phaseB.dacValue12Bits);

    TB67H450_SetInputA(true, true);
    TB67H450_SetInputB(true, true);
}

void TB67H450_SetTwoCoilsCurrent(unsigned short _currentA_3300mAIn12Bits, unsigned short _currentB_3300mAIn12Bits)
{
    /*
     * After SetFocCurrentVector calculation a 12bits value was mapped to 0~3300mA.
     * And due to used 0.1Ohm shank resistor, 0~3300mV V-ref means 0~3300mA CurrentSetPoint,
     * For more details, see TB67H450 Datasheet page.10 .
     */

    TB67H450_DacOutputVoltage(_currentA_3300mAIn12Bits, _currentB_3300mAIn12Bits);
}

void TB67H450_SetFocCurrentVector(unsigned int _directionInCount, int _current_mA)
{
    // 由细分数获得数组指针
    phaseB.sinMapPtr = (_directionInCount) & (0x000003FF); //对 1024 取余（0x3FF == 1024）
    phaseA.sinMapPtr = (phaseB.sinMapPtr + (256)) & (0x000003FF); //对 1024 取余（0x3FF == 1024）
    
    // 由数据指针获得整形数据（空间换时间方案）
    phaseA.sinMapData = sin_pi_m2[phaseA.sinMapPtr];
    phaseB.sinMapData = sin_pi_m2[phaseB.sinMapPtr];

    // 由整形数据获得 DAC 寄存器数据（即电压）
    unsigned int dacValue12Bits = abs(_current_mA); // 电压电流关系为 1:1（检流电阻为 0.1 欧）
    
    // 将数据扩大 4096 倍 (dacValue12Bits * 4096 * 4095 / 3300) / 4096) 其中 (4096 * 4095 / 3300) = 5083，右移 12 位，即除以 4096 将数据缩小 4096 倍。
    dacValue12Bits = (unsigned int)(dacValue12Bits * 5083) >> 12; // (dacValue12Bits * 4095 / 3300) 的变形
    dacValue12Bits = dacValue12Bits & (0x00000FFF); // 对 4096 取余，向小取整，舍弃符号位

    phaseA.dacValue12Bits = (unsigned int)(dacValue12Bits * abs(phaseA.sinMapData)) >> sin_pi_m2_dpiybit; // sin_1024_dpiy 的变种
    phaseB.dacValue12Bits = (unsigned int)(dacValue12Bits * abs(phaseB.sinMapData)) >> sin_pi_m2_dpiybit; // sin_1024_dpiy 的变种

    // DAC 输出
    TB67H450_SetTwoCoilsCurrent(phaseA.dacValue12Bits, phaseB.dacValue12Bits);
    
    if (phaseA.sinMapData > 0)
        TB67H450_SetInputA(true, false);
    else if (phaseA.sinMapData < 0)
        TB67H450_SetInputA(false, true);
    else
        TB67H450_SetInputA(true, true);

    if (phaseB.sinMapData > 0)
        TB67H450_SetInputB(true, false);
    else if (phaseB.sinMapData < 0)
        TB67H450_SetInputB(false, true);
    else
        TB67H450_SetInputB(true, true);
}

/**
 * TB67H450 做到 256 细分的原理：
 * TB67H450 支持使用电压参考点来设置 TB67H450 的输出电流，而为了做到细分控制需要控制 TB67H450 输出电流呈 sine 函数的变化规律。
 * 并且将电流的变化梯度分割为 256 个变化梯度。
 * 所以只要提供给 TB67H450 的参考电压遵循 sine 函数的变化规律，TB67H450 的输出电流也就呈现 sine 函数的变化规律，并且将参考电压
 * 分割为 256 个梯度，即 将 0-3.3V 分割为 256 份。
 * 
 * 而控制电压的变化可以采用查 sine 函数表的方式，从表中查出相应值后换算为 PWM 或 DAC 的电压输出。 
 */
